#include "string.h"
#include "M2M_Serial_Console.h"
#include "USART2_DMA.h"
#include "NumTOStr.h"
#include "servo.h"
#include "Justin_Beeper.h"
#include "led.h"
#include "pstwo.h"
#include "motor.h"
u8 *M2MSerial_Address;
u8 M2MSerial_RxWaiting=0;//接收指令等待标志位
u8 M2MSerial_TxMode=0;//发送模式
u16 M2MSerial_RxWaitingTim=0;//接收延时计数器
u8 OK_Respond[]="JHYTEST OK\r\n";//正常情况下引导信号、正常接收到修改指令的应答
u8 ERR_Respond[]="JHYTESTERR\r\n";//收到错误指令的答复
u8 M2MSerial_RxCmd[M2MSerial_CmdUnit+1]="\0";//接收容器
u8 M2MSerial_TxCmd[M2MSerial_CmdUnit+1]="J";//发送容器
void M2MSerial_Control()
{
    if(USART2_RX_FLAG)
    {
        M2MSerial_RxWaiting=0;
        M2MSerial_RxToCmd();
        M2MSerial_RxWaitingTim=0;
        M2MSerial_RxWaiting=1;
        USART2_RX_FLAG=0;//标志位清零
    }
    M2MSerial_ConnectionStatueCheck();
}
void M2MSerial_RxToCmd(void)
{
    int i,M2MSerial_HeatBeat=0;
    //板间通讯心跳包（引导信号）判定
    for(i=0;i<M2MSerial_CmdUnit-2;i++)//后两位是校验
    {
        if(M2MSerial_RxCmd[i]==OK_Respond[i])
        {
            M2MSerial_HeatBeat=1;
        }
        else
        {
            M2MSerial_HeatBeat=0;
            break;
        }
    }

    if(M2MSerial_HeatBeat)//是心跳包
    {
        M2MSerial_CmdToTx();//返回对底盘电机的控制参数（或者要加上舵机的参数读取）
        return;
    }
    else if(M2MSerial_RxCmd[0]=='J')//不是普通的引导指令，是对副控制板相关参数的修改指令
    {
        switch (M2MSerial_RxCmd[1])//控制位检查
        {
        case 0x03://控制位信息：副控制板参数修改
            switch (M2MSerial_RxCmd[2])//标志位1检查
            {
            
            case 0x01://servo speed control
                switch (M2MSerial_RxCmd[3])
                {
                case 0x01:
                    SpeedRatio[1]=StrtoNum_4digit(M2MSerial_RxCmd,4)/100.0;
                    SpeedRatio[2]=StrtoNum_4digit(M2MSerial_RxCmd,6)/100.0;
                    break;
                case 0x02:
                    SpeedRatio[3]=StrtoNum_4digit(M2MSerial_RxCmd,4)/100.0;
                    SpeedRatio[4]=StrtoNum_4digit(M2MSerial_RxCmd,6)/100.0;
                    break;
                default:
                    break;
                }
                DMA_USART2_Tx_Data(OK_Respond,M2MSerial_CmdUnit);
                break;
            case 0x02://beep switch
                Flag_MuteLock=M2MSerial_RxCmd[3];
                DMA_USART2_Tx_Data(OK_Respond,M2MSerial_CmdUnit);
                break;
            default:
                DMA_USART2_Tx_Data(ERR_Respond,M2MSerial_CmdUnit);//没有对应的修改指令
                break;
            }
            break;
        default:
            M2MSerial_Address=ERR_Respond;
            DMA_USART2_Tx_Data(M2MSerial_Address,M2MSerial_CmdUnit);//控制位指令错误
            break;
        }
        
    }
    else//帧头帧尾校验出错
    {
        M2MSerial_Address=ERR_Respond;
        DMA_USART2_Tx_Data(M2MSerial_Address,M2MSerial_CmdUnit);//控制位指令错误
    }
}
void M2MSerial_CmdToTx(void)
{
    u8 MainMotor_Cmd=MainMotor_Control();
    switch (M2MSerial_TxMode)
    {
    case 0:
        M2MSerial_TxCmd[1]='A';
        M2MSerial_TxCmd[2]=MainMotor_Cmd>>4;//底盘电机方向
        M2MSerial_TxCmd[3]=MainMotor_Cmd&0x0f;//底盘电机速度
        NumtoStr_4digit((int)PS2_AnologData(PSS_RX),M2MSerial_TxCmd,4);
        NumtoStr_4digit((int)PS2_AnologData(PSS_RY),M2MSerial_TxCmd,6);
        M2MSerial_TxCmd[8]=0;
        M2MSerial_TxCmd[9]=0;
        break;
    case 1:
        M2MSerial_TxCmd[1]='A';
        NumtoStr_4digit((int)(SpeedRatio[1]*100.0),M2MSerial_TxCmd,2);
        NumtoStr_4digit((int)(SpeedRatio[2]*100.0),M2MSerial_TxCmd,4);
        NumtoStr_4digit((int)(SpeedRatio[3]*100.0),M2MSerial_TxCmd,6);
        NumtoStr_4digit((int)(SpeedRatio[4]*100.0),M2MSerial_TxCmd,8);
        break;
    default:
        break;
    }
    DMA_USART2_Tx_Data(M2MSerial_TxCmd,M2MSerial_CmdUnit);
}
void M2MSerial_ConnectionStatueCheck(void)
{
    
}
void M2MSerial_ErrorReaction(void)
{

}
